package com.example.geoaimavlink_android.activity.fragment

import android.os.Bundle
import android.view.LayoutInflater
import android.view.View
import android.view.ViewGroup
import android.widget.Toast
import com.example.geoaimavlink_android.R
import com.example.geoaimavlink_android.activity.view.virtualstick.OnScreenJoystick
import com.example.geoaimavlink_android.activity.view.virtualstick.OnScreenJoystickListener
import com.geoai.mavlink.geoainet.base.mavlinkcore.interfaces.CompletionCallback.ICompletionCallback
import kotlinx.android.synthetic.main.frag_virtual_stick.btn_disable_virtual_stick
import kotlinx.android.synthetic.main.frag_virtual_stick.btn_enable_virtual_stick
import kotlinx.android.synthetic.main.frag_virtual_stick.btn_gohome
import kotlinx.android.synthetic.main.frag_virtual_stick.btn_landing
import kotlinx.android.synthetic.main.frag_virtual_stick.btn_take_off
import kotlinx.android.synthetic.main.frag_virtual_stick.left_stick_view
import kotlinx.android.synthetic.main.frag_virtual_stick.right_stick_view
import kotlinx.android.synthetic.main.frag_virtual_stick.virtual_stick_info_tv
import java.util.concurrent.Executors
import java.util.concurrent.ScheduledFuture
import java.util.concurrent.TimeUnit
import java.util.concurrent.atomic.AtomicBoolean
import kotlin.math.pow
import kotlin.math.sqrt

class VirtualStickFragment : BaseFragment() {

    private val isVirtualStickEnable = AtomicBoolean(false)
    private var axisX: Float = 0.0f
    private var axisY: Float = 0.0f
    private var axisZ: Float = 0.0f
    private var axisR: Float = 0.0f
    private var joyStickThread: ScheduledFuture<*>? = null

    override fun onCreateView(
        inflater: LayoutInflater,
        container: ViewGroup?,
        savedInstanceState: Bundle?
    ): View? {
        return inflater.inflate(R.layout.frag_virtual_stick, container, false)
    }

    override fun onViewCreated(view: View, savedInstanceState: Bundle?) {
        super.onViewCreated(view, savedInstanceState)

        btn_take_off.setOnClickListener {
            getProduct()?.flyControllerManager?.startAircraftTakeoff(
                handleCompletionCallback()
            )
        }
        btn_landing.setOnClickListener {
            getProduct()?.flyControllerManager?.startForceLanding(
                handleCompletionCallback()
            )
        }
        btn_gohome.setOnClickListener {
            getProduct()?.flyControllerManager?.startAircraftGoHome(
                handleCompletionCallback()
            )
        }
        btn_enable_virtual_stick.setOnClickListener {
            getProduct()?.flyControllerManager?.setNewJoyStickEnable(true, handleCompletionCallback())
            isVirtualStickEnable.compareAndSet(false, true)
        }
        btn_disable_virtual_stick.setOnClickListener {
            getProduct()?.flyControllerManager?.setNewJoyStickEnable(false, handleCompletionCallback())
            isVirtualStickEnable.compareAndSet(true, false)
        }

        right_stick_view.setJoystickListener(object : OnScreenJoystickListener {
            override fun onTouch(joystick: OnScreenJoystick?, pX: Float, pY: Float) {
                axisX = (pX * 5.0f)
                axisY = (pY * 5.0f)
            }
        })

        left_stick_view.setJoystickListener(object : OnScreenJoystickListener {
            override fun onTouch(joystick: OnScreenJoystick?, pX: Float, pY: Float) {
                axisR = (pX * 5.0f)
                axisZ = (pY * 3.0f)
            }
        })

        joyStickThread = Executors.newSingleThreadScheduledExecutor().scheduleWithFixedDelay({

            updateVirtualStickInfo()

            if (isVirtualStickEnable.get().not()) return@scheduleWithFixedDelay

            getProduct()?.flyControllerManager?.sendNewJoyStickControl(
                axisY,
                axisX,
                -axisZ,
                axisR,
                null
            )
        }, 100, 100, TimeUnit.MILLISECONDS)
    }

    private fun handleCompletionCallback(): ICompletionCallback {
        return ICompletionCallback {
            mainHandler.post {
                if (it == null) {
                    Toast.makeText(context, "success", Toast.LENGTH_SHORT).show()
                } else {
                    Toast.makeText(context, "failure: " + it.description, Toast.LENGTH_SHORT).show()
                }
            }
        }
    }

    private fun updateVirtualStickInfo() {
        val vsSpeed = getProduct()?.flyControllerManager?.flyControllerInfo?.let {
            sqrt(it.aircraftVelocityX.toDouble().pow(2.0) + it.aircraftVelocityY.toDouble().pow(2.0))
        } ?: 0
        val hsSpeed = getProduct()?.flyControllerManager?.flyControllerInfo?.let {
            it.aircraftVelocityZ.times(-1)
        } ?: 0

        val builder = StringBuilder()
        builder.append("Is virtual stick enable:").append(isVirtualStickEnable.get())
        builder.append("\n")
        builder.append("Is Flying:").append(getProduct()?.flyControllerManager?.flyControllerInfo?.isFlying)
        builder.append("\n")
        builder.append("Horizon Speed:").append(String.format("%.2f", hsSpeed))
        builder.append("\n")
        builder.append("Vertical Speed:").append(String.format("%.2f", vsSpeed))
        builder.append("\n")
        builder.append("Altitude:").append(getProduct()?.flyControllerManager?.flyControllerInfo?.aircraftAltitude)
        builder.append("\n")
        builder.append("Yaw:").append(getProduct()?.flyControllerManager?.flyControllerInfo?.aircraftYaw)
        builder.append("\n")
        builder.append("Fc Mode:").append(getProduct()?.flyControllerManager?.flyControllerInfo?.flyMode?.name)
        builder.append("\n")
        mainHandler.post {
            if (virtual_stick_info_tv != null) virtual_stick_info_tv.text = builder.toString()
        }
    }
}